﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Timers;
using System.Threading;

namespace SCI_actuator
{
    public partial class HandMovement : Form
    {
        private menu_control frmain;
        private bool flag_start;    //启动标志
        private byte position_h1;    //记录舵机1位置高位
        private byte position_l1;     //记录舵机1位置地位
        private byte position_h2;    //记录舵机2位置高位
        private byte position_l2;     //记录舵机2位置地位
        private byte position_h3;    //记录舵机3位置高位
        private byte position_l3;     //记录舵机3位置地位
        private byte position_h4;    //记录舵机4位置高位
        private byte position_l4;     //记录舵机4位置地位
        private bool flag_stop1;    //舵机1停止标志
        private bool flag_stop2;    //舵机2停止标志
        private bool flag_stop3;    //舵机3停止标志
        private bool flag_stop4;    //舵机4停止标志
        private int position_min1;   //舵机1位置最小值
        private int position_max1;   //舵机1位置最大值
        private int position_min2;   //舵机2位置最小值
        private int position_max2;   //舵机2位置最大值
        private int position_min3;   //舵机3位置最小值
        private int position_max3;   //舵机3位置最大值
        private int position_min4;   //舵机4位置最小值
        private int position_max4;   //舵机4位置最大值
        private int delay_time;      //舵机每次控制延迟时间

        public HandMovement(menu_control fr)
        {
            InitializeComponent();

            this.flag_start = false;
            this.frmain = fr;
            this.flag_stop1 = false;
            this.flag_stop2 = false;
            this.flag_stop3 = false;
            this.flag_stop4 = false;
            this.position_max1 = 1023;
            this.position_min1 = 0;
            this.position_max2 = 832;
            this.position_min2 = 320;
            this.position_max3 = 880;
            this.position_min3 = 256;
            this.position_max4 = 880;
            this.position_min4 = 256;
            this.delay_time = 50;

            buttonEngine1Left.Enabled = false;
            buttonEngine1Right.Enabled = false;
            buttonEngine2Left.Enabled = false;
            buttonEngine2Right.Enabled = false;
            buttonEngine3Left.Enabled = false;
            buttonEngine3Right.Enabled = false;
            buttonEngine4Left.Enabled = false;
            buttonEngine4Right.Enabled = false;
        }

        private void Delay_ms(int mm)
        {
            DateTime current = DateTime.Now;

            while (current.AddMilliseconds(mm) > DateTime.Now)
            {
                Application.DoEvents();
            }
            return;
        }

        private void buttonExit_Click(object sender, EventArgs e)
        {
            flag_start = false;   //重置标志位

            this.Hide();
            this.Dispose();
        }

        private void buttonStart_Click(object sender, EventArgs e)
        {
            string sum_str = null;
            byte[] bRx = this.frmain.HexStringToByteArray("FE" + "11"    //计算检验和
                + "01" + "02" + constant.REG_ACTION + "01"
                + "02" + "02" + constant.REG_ACTION + "01"
                + "03" + "02" + constant.REG_ACTION + "01"
                + "04" + "02" + constant.REG_ACTION + "01");
            byte sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            string senddata = "FF" + "FF" + "FE" + "11"
                + "01" + "02" + constant.REG_ACTION + "01"
                + "02" + "02" + constant.REG_ACTION + "01"
                + "03" + "02" + constant.REG_ACTION + "01"
                + "04" + "02" + constant.REG_ACTION + "01" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();
            }
            this.frmain.servosend(senddata);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }
            Delay_ms(100);

            bRx = this.frmain.HexStringToByteArray("FE" + "19"           //实时曲线
                + "01" + "04" + constant.REG_SEEK_POSITION_HI + "01" + constant.REG_SEEK_POSITION_LO + "C4"
                + "02" + "04" + constant.REG_SEEK_POSITION_HI + "02" + constant.REG_SEEK_POSITION_LO + "2C"
                + "03" + "04" + constant.REG_SEEK_POSITION_HI + "02" + constant.REG_SEEK_POSITION_LO + "2C"
                + "04" + "04" + constant.REG_SEEK_POSITION_HI + "02" + constant.REG_SEEK_POSITION_LO + "2C");
            sum = this.frmain.checksum(bRx);

            if (sum >= 16)
                sum_str = Convert.ToString(sum, 16);
            else
                sum_str = "0" + Convert.ToString(sum, 16);
            senddata = "FF" + "FF" + "FE" + "19"
                + "01" + "04" + constant.REG_SEEK_POSITION_HI + "01" + constant.REG_SEEK_POSITION_LO + "C4"
                + "02" + "04" + constant.REG_SEEK_POSITION_HI + "02" + constant.REG_SEEK_POSITION_LO + "2C"
                + "03" + "04" + constant.REG_SEEK_POSITION_HI + "02" + constant.REG_SEEK_POSITION_LO + "2C"
                + "04" + "04" + constant.REG_SEEK_POSITION_HI + "02" + constant.REG_SEEK_POSITION_LO + "2C" + sum_str;

            if (this.frmain.send_timer.Enabled == true)
            {
                this.frmain.send_timer.Stop();

            }
            this.frmain.servosend(senddata);
            if (this.frmain.realtime_judge == false)
            {
                this.frmain.send_timer.Stop();
            }
            else if (this.frmain.realtime_judge == true)
            {
                this.frmain.send_timer.Start();
            }

            flag_start = true;    //置位标志位，表示已经初始化
            position_h1 = Convert.ToByte("01", 16);
            position_l1 = Convert.ToByte("C4", 16);
            position_h2 = Convert.ToByte("02", 16);
            position_l2 = Convert.ToByte("2C", 16);
            position_h3 = Convert.ToByte("02", 16);
            position_l3 = Convert.ToByte("2C", 16);
            position_h4 = Convert.ToByte("02", 16);
            position_l4 = Convert.ToByte("2C", 16);

            buttonEngine1Left.Enabled = true;
            buttonEngine1Right.Enabled = true;
            buttonEngine2Left.Enabled = true;
            buttonEngine2Right.Enabled = true;
            buttonEngine3Left.Enabled = true;
            buttonEngine3Right.Enabled = true;
            buttonEngine4Left.Enabled = true;
            buttonEngine4Right.Enabled = true;
        }

        private void buttonEngine1Left_MouseDown(object sender, MouseEventArgs e)
        {
            string output_left1 = "舵机1 向左：";
            string output_left1_stop = "舵机1 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l1 < 4)   //借位
                {
                    if (position_h1 != 0)
                    {
                        position_h1 = (byte)(position_h1 - 1);
                    }
                    else
                    {
                        position_l1 = 4;
                    }
                }
                position_l1 = (byte)(position_l1 - 4);

                position = position_h1 * 256 + position_l1;

                if (position < position_min1)    //限幅
                {
                    position_h1 = (byte)(position_min1 / 256);
                    position_l1 = (byte)(position_min1 % 256);
                }

                if (position > position_max1)     //限幅
                {
                    position_h1 = (byte)(position_max1 / 256);
                    position_l1 = (byte)(position_max1 % 256);
                }

                if (position_h1 >= 16)
                    pos_h = (Convert.ToString(position_h1, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h1, 16)).ToUpper();

                if (position_l1 >= 16)
                    pos_l = (Convert.ToString(position_l1, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l1, 16)).ToUpper();

                textBoxOutput.Text = output_left1 + pos_h + pos_l;

                //串口发送数据 
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h1 >= 16)
                    p_h = (Convert.ToString(position_h1, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h1, 16)).ToUpper();

                if (position_l1 >= 16)
                    p_l = (Convert.ToString(position_l1, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l1, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "01" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "01" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止
                if (flag_stop1)
                {
                    flag_stop1 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_left1_stop + pos_h + pos_l;

        }

        private void buttonEngine1Left_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop1 = true;
        }

        private void buttonEngine1Right_MouseDown(object sender, MouseEventArgs e)
        {
            string output_right1 = "舵机1 向右：";
            string output_right1_stop = "舵机1 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l1 > 251)
                {
                    position_h1 = (byte)(position_h1 + 1);
                }
                position_l1 = (byte)(position_l1 + 4);

                position = position_h1 * 256 + position_l1;

                if (position < position_min1)    //限幅
                {
                    position_h1 = (byte)(position_min1 / 256);
                    position_l1 = (byte)(position_min1 % 256);
                }

                if (position > position_max1)     //限幅
                {
                    position_h1 = (byte)(position_max1 / 256);
                    position_l1 = (byte)(position_max1 % 256);
                }

                if (position_h1 >= 16)
                    pos_h = (Convert.ToString(position_h1, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h1, 16)).ToUpper();

                if (position_l1 >= 16)
                    pos_l = (Convert.ToString(position_l1, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l1, 16)).ToUpper();

                textBoxOutput.Text = output_right1 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h1 >= 16)
                    p_h = (Convert.ToString(position_h1, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h1, 16)).ToUpper();

                if (position_l1 >= 16)
                    p_l = (Convert.ToString(position_l1, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l1, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "01" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "01" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止
                if (flag_stop1)
                {
                    flag_stop1 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_right1_stop + pos_h + pos_l;
        }

        private void buttonEngine1Right_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop1 = true;
        }


        private void buttonEngine2Left_MouseDown(object sender, MouseEventArgs e)
        {
            string output_left2 = "舵机2 向左：";
            string output_left2_stop = "舵机2 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l2 < 4)   //借位
                {
                    if (position_h2 != 0)
                    {
                        position_h2 = (byte)(position_h2 - 1);
                    }
                    else
                    {
                        position_l2 = 4;
                    }
                }
                position_l2 = (byte)(position_l2 - 4);

                position = position_h2 * 256 + position_l2;

                if (position < 304)    //限幅
                {
                    position_h2 = (byte)(position_min2 / 256);
                    position_l2 = (byte)(position_min2 % 256);
                }

                if (position > 832)     //限幅
                {
                    position_h2 = (byte)(position_max2 / 256);
                    position_l2 = (byte)(position_max2 % 256);
                }

                if (position_h2 >= 16)
                    pos_h = (Convert.ToString(position_h2, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h2, 16)).ToUpper();

                if (position_l2 >= 16)
                    pos_l = (Convert.ToString(position_l2, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l2, 16)).ToUpper();

                textBoxOutput.Text = output_left2 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h2 >= 16)
                    p_h = (Convert.ToString(position_h2, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h2, 16)).ToUpper();

                if (position_l2 >= 16)
                    p_l = (Convert.ToString(position_l2, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l2, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "02" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "02" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止
                if (flag_stop2)
                {
                    flag_stop2 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_left2_stop + pos_h + pos_l;
        }

        private void buttonEngine2Left_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop2 = true;
        }

        private void buttonEngine2Right_MouseDown(object sender, MouseEventArgs e)
        {
            string output_right2 = "舵机2 向右：";
            string output_right2_stop = "舵机2 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l2 > 251)
                {
                    position_h2 = (byte)(position_h2 + 1);
                }
                position_l2 = (byte)(position_l2 + 4);

                position = position_h2 * 256 + position_l2;

                if (position < 304)    //限幅
                {
                    position_h2 = (byte)(position_min2 / 256);
                    position_l2 = (byte)(position_min2 % 256);
                }

                if (position > 832)     //限幅
                {
                    position_h2 = (byte)(position_max2 / 256);
                    position_l2 = (byte)(position_max2 % 256);
                }

                if (position_h2 >= 16)
                    pos_h = (Convert.ToString(position_h2, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h2, 16)).ToUpper();

                if (position_l2 >= 16)
                    pos_l = (Convert.ToString(position_l2, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l2, 16)).ToUpper();

                textBoxOutput.Text = output_right2 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h2 >= 16)
                    p_h = (Convert.ToString(position_h2, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h2, 16)).ToUpper();

                if (position_l2 >= 16)
                    p_l = (Convert.ToString(position_l2, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l2, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "02" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "02" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止

                if (flag_stop2)
                {
                    flag_stop2 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_right2_stop + pos_h + pos_l;
        }

        private void buttonEngine2Right_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop2 = true;
        }

        private void buttonEngine3Left_MouseDown(object sender, MouseEventArgs e)
        {
            string output_left3 = "舵机3 向左：";
            string output_left3_stop = "舵机3 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l3 < 4)   //借位
                {
                    if (position_h3 != 0)
                    {
                        position_h3 = (byte)(position_h3 - 1);
                    }
                    else
                    {
                        position_l3 = 4;
                    }
                }
                position_l3 = (byte)(position_l3 - 4);

                position = position_h3 * 256 + position_l3;

                if (position < position_min3)    //限幅
                {
                    position_h3 = (byte)(position_min3 / 256);
                    position_l3 = (byte)(position_min3 % 256);
                }

                if (position > position_max3)     //限幅
                {
                    position_h3 = (byte)(position_max3 / 256);
                    position_l3 = (byte)(position_max3 % 256);
                }

                if (position_h3 >= 16)
                    pos_h = (Convert.ToString(position_h3, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h3, 16)).ToUpper();

                if (position_l3 >= 16)
                    pos_l = (Convert.ToString(position_l3, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l3, 16)).ToUpper();

                textBoxOutput.Text = output_left3 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h3 >= 16)
                    p_h = (Convert.ToString(position_h3, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h3, 16)).ToUpper();

                if (position_l3 >= 16)
                    p_l = (Convert.ToString(position_l3, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l3, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "03" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "03" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止

                if (flag_stop3)
                {
                    flag_stop3 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_left3_stop + pos_h + pos_l;
        }

        private void buttonEngine3Left_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop3 = true;
        }

        private void buttonEngine3Right_MouseDown(object sender, MouseEventArgs e)
        {
            string output_right3 = "舵机3 向右：";
            string output_right3_stop = "舵机3 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l3 > 251)
                {
                    position_h3 = (byte)(position_h3 + 1);
                }
                position_l3 = (byte)(position_l3 + 4);

                position = position_h3 * 256 + position_l3;

                if (position < position_min3)    //限幅
                {
                    position_h3 = (byte)(position_min3 / 256);
                    position_l3 = (byte)(position_min3 % 256);
                }

                if (position > position_max3)     //限幅
                {
                    position_h3 = (byte)(position_max3 / 256);
                    position_l3 = (byte)(position_max3 % 256);
                }

                if (position_h3 >= 16)
                    pos_h = (Convert.ToString(position_h3, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h3, 16)).ToUpper();

                if (position_l3 >= 16)
                    pos_l = (Convert.ToString(position_l3, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l3, 16)).ToUpper();

                textBoxOutput.Text = output_right3 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h3 >= 16)
                    p_h = (Convert.ToString(position_h3, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h3, 16)).ToUpper();

                if (position_l3 >= 16)
                    p_l = (Convert.ToString(position_l3, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l3, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "03" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "03" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止

                if (flag_stop3)
                {
                    flag_stop3 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_right3_stop + pos_h + pos_l;
        }

        private void buttonEngine3Right_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop3 = true;
        }

        private void buttonEngine4Left_MouseDown(object sender, MouseEventArgs e)
        {
            string output_left4 = "舵机4 向左：";
            string output_left4_stop = "舵机4 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l4 < 4)   //借位
                {
                    if (position_h4 != 0)
                    {
                        position_h4 = (byte)(position_h4 - 1);
                    }
                    else
                    {
                        position_l4 = 4;
                    }
                }
                position_l4 = (byte)(position_l4 - 4);

                position = position_h4 * 256 + position_l4;

                if (position < position_min4)    //限幅
                {
                    position_h4 = (byte)(position_min4 / 256);
                    position_l4 = (byte)(position_min4 % 256);
                }

                if (position > position_max4)     //限幅
                {
                    position_h4 = (byte)(position_max4 / 256);
                    position_l4 = (byte)(position_max4 % 256);
                }

                if (position_h4 >= 16)
                    pos_h = (Convert.ToString(position_h4, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h4, 16)).ToUpper();

                if (position_l4 >= 16)
                    pos_l = (Convert.ToString(position_l4, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l4, 16)).ToUpper();

                textBoxOutput.Text = output_left4 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h4 >= 16)
                    p_h = (Convert.ToString(position_h4, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h4, 16)).ToUpper();

                if (position_l4 >= 16)
                    p_l = (Convert.ToString(position_l4, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l4, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "04" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "04" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止

                if (flag_stop4)
                {
                    flag_stop4 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_left4_stop + pos_h + pos_l;
        }

        private void buttonEngine4Left_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop4 = true;
        }

        private void buttonEngine4Right_MouseDown(object sender, MouseEventArgs e)
        {
            string output_right4 = "舵机4 向右：";
            string output_right4_stop = "舵机4 停止：";
            int position = 0;
            string pos_h = "";
            string pos_l = "";

            while (flag_start)
            {
                if (position_l4 > 251)
                {
                    position_h4 = (byte)(position_h4 + 1);
                }
                position_l4 = (byte)(position_l4 + 4);

                position = position_h4 * 256 + position_l4;

                if (position < position_min4)    //限幅
                {
                    position_h4 = (byte)(position_min4 / 256);
                    position_l4 = (byte)(position_min4 % 256);
                }

                if (position > position_max4)     //限幅
                {
                    position_h4 = (byte)(position_max4 / 256);
                    position_l4 = (byte)(position_max4 % 256);
                }

                if (position_h4 >= 16)
                    pos_h = (Convert.ToString(position_h4, 16)).ToUpper();
                else
                    pos_h = ("0" + Convert.ToString(position_h4, 16)).ToUpper();

                if (position_l4 >= 16)
                    pos_l = (Convert.ToString(position_l4, 16)).ToUpper();
                else
                    pos_l = ("0" + Convert.ToString(position_l4, 16)).ToUpper();

                textBoxOutput.Text = output_right4 + pos_h + pos_l;

                //串口发送数据
                string sum_str = null;
                string p_h = "";
                string p_l = "";

                if (position_h4 >= 16)
                    p_h = (Convert.ToString(position_h4, 16)).ToUpper();
                else
                    p_h = ("0" + Convert.ToString(position_h4, 16)).ToUpper();

                if (position_l4 >= 16)
                    p_l = (Convert.ToString(position_l4, 16)).ToUpper();
                else
                    p_l = ("0" + Convert.ToString(position_l4, 16)).ToUpper();

                byte[] bRx = this.frmain.HexStringToByteArray(
                    "04" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l);
                byte sum = this.frmain.checksum(bRx);

                if (sum >= 16)
                    sum_str = Convert.ToString(sum, 16);
                else
                    sum_str = "0" + Convert.ToString(sum, 16);
                string senddata = "FF" + "FF" +
                    "04" + "06" + "02" + constant.REG_SEEK_POSITION_HI + p_h + constant.REG_SEEK_POSITION_LO + p_l + sum_str;

                if (this.frmain.send_timer.Enabled == true)
                {
                    this.frmain.send_timer.Stop();

                }
                this.frmain.servosend(senddata);
                if (this.frmain.realtime_judge == false)
                {
                    this.frmain.send_timer.Stop();
                }
                else if (this.frmain.realtime_judge == true)
                {
                    this.frmain.send_timer.Start();
                }

                //判断是否需要停止

                if (flag_stop4)
                {
                    flag_stop4 = false;
                    break;
                }

                Delay_ms(delay_time);
            }

            textBoxOutput.Text = output_right4_stop + pos_h + pos_l;
        }

        private void buttonEngine4Right_MouseUp(object sender, MouseEventArgs e)
        {
            flag_stop4 = true;
        }

    }
}
